#include "motor.h"
/*
函数功能：电机取绝对值函数
输入：
    int p   输入的pwm值(有正负)
输出：
    int q   输出的pwm(无正负)
备注：无
*/
int GFP_abs(int p)
{
	int q;
	q=p>0?p:(-p);
	return q;
}
/*
函数功能：电机限幅函数
输入：
    int *motoA：电机pwm1
    int *motoB：电机pwm2
输出：无
备注：限制在PWM_MIN至PWM_MAX之间
*/
void Limit(int *motoA,int *motoB)
{
	if(*motoA>PWM_MAX)*motoA=PWM_MAX;
	if(*motoA<PWM_MIN)*motoA=PWM_MIN;
	
	if(*motoB>PWM_MAX)*motoB=PWM_MAX;
	if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}
/*
函数功能：电机初始化
输入：无
输出：无
备注：初始化电机的逻辑口及pwm初始化
*/
void    motor_init(void)
{
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P3, GPIO_PIN5|GPIO_PIN6|GPIO_PIN7);
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P5, GPIO_PIN2);
    TimA2_pwm_init(1,7200);
    MOTOR_PIN1 =0;
    MOTOR_PIN2 =0;
    MOTOR_PIN3 =0;
    MOTOR_PIN4 =0;
}
/*
函数功能：电机装载函数
输入：  
        int     motor1：电机1的pwm(有正负)
        int     motor2：电机2的pwm(有正负)
输出：无
备注：电机装载函数，可直接调用驱动电机
    motor1:逻辑口   P3.5  P3.6     PWM1输出口   P5.6
    motor2:逻辑口   P3.7  P5.2     PWM2输出口   P5.7 

    MOTOR_PIN1      P3.5
    MOTOR_PIN2      P3.6

    MOTOR_PIN3      P3.7
    MOTOR_PIN4      P5.2

*/
void    Load    (int   motor1,int motor2)
{
    Limit(&motor1,&motor2);//电机限幅
    if(motor1>=0)
    {
        MOTOR_PIN1=1;
        MOTOR_PIN2=0;
        MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,GFP_abs(motor1));
    }
    else 
    {
        MOTOR_PIN1=0;
        MOTOR_PIN2=1;
        MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,GFP_abs(motor1));
    }
    if(motor2>=0)
    {
        MOTOR_PIN3=1;
        MOTOR_PIN4=0;
        MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2,GFP_abs(motor2));
    }
    else 
    {
        MOTOR_PIN3=0;
        MOTOR_PIN4=1;
        MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2,GFP_abs(motor2));
    }
}



